package com.hitqz.robot.biz.business.dispatch.impl;

import cn.hutool.extra.spring.SpringUtil;
import com.baomidou.mybatisplus.core.toolkit.Wrappers;
import com.hitqz.robot.api.business.enums.RobotDriverEnum;
import com.hitqz.robot.api.common.dto.robot.RobotGoTargetPosDto;
import com.hitqz.robot.api.common.dto.warning.AddWarningDto;
import com.hitqz.robot.api.common.entity.MapArea;
import com.hitqz.robot.api.common.entity.Robot;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.api.dispatch.protocol.dto.RobotMapPosDto;
import com.hitqz.robot.biz.business.dispatch.DispatchService;
import com.hitqz.robot.biz.service.RobotMapPosService;
import com.hitqz.robot.biz.service.RobotService;
import com.hitqz.robot.driver.model.IRobotPosDto;
import com.hitqz.robot.driver.parent.RobotDriverService;
import com.hitqz.robot.netty.service.Send2ServerService;
import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.locationtech.jts.geom.Coordinate;
import org.locationtech.jts.geom.GeometryFactory;
import org.locationtech.jts.geom.Point;
import org.locationtech.jts.operation.distance.DistanceOp;
import org.springframework.stereotype.Service;

import java.util.LinkedList;
import java.util.List;
import java.util.Objects;
import java.util.UUID;

/**
 * @author xupkun
 * @date 2024/6/6
 */
@Service
@RequiredArgsConstructor
@Slf4j
public class DispatchServiceImpl implements DispatchService {

    private final RobotMapPosService robotMapPosService;

    private final RobotService robotService;

    private final RobotDriverService robotDriverService;

    private final Send2ServerService send2ServerService;

    @Override
    public void initPathAndPos(List<RobotMapPos> mapPos, List<MapArea> mapAreas) {

    }




    @Override
    public boolean go2TargetPosition(RobotMapPos mapPos, boolean charge) {
        Robot robot = robotService.getDefaultRobot();
        try {
            //自研的导航可以切地图
            if (!isSameMap(robot, mapPos) && robotDriverService.getDriverVersion()!=RobotDriverEnum.HIT) {
                throw new IllegalStateException("目标位置的地图代码 " + mapPos.getMapCode() + " 与机器人当前所在地图代码 " + robot.getMapCode() + " 不一致");
            }
            //计算数据库中最近点 todo 距离超过限制的要舍弃或抛出异常
            List<RobotMapPos> currentMapPositions = robotMapPosService.list(Wrappers.<RobotMapPos>lambdaQuery()
                    .eq(RobotMapPos::getMapCode, robot.getMapCode()));
            IRobotPosDto posDto = robotDriverService.getWebPos();
            RobotMapPos nearestPosition = findNearestPosition(posDto, currentMapPositions);
            //获取地图路径点
            List<RobotMapPos> allPositions = currentMapPositions;
            if (robotDriverService.getDriverVersion()==RobotDriverEnum.HIT){
                allPositions = robotMapPosService.list();
            }
            List<RobotMapPos> path = robotDriverService.getNavPath(nearestPosition, mapPos, allPositions);
            robotDriverService.robotTaskGoTarget(
                    RobotGoTargetPosDto.builder()
                            .robot(robot)
                            .code(mapPos.getCode())
                            .charge(charge)
                            .posId(mapPos.getId().intValue())
                            .taskId(UUID.randomUUID().toString().replace("-", ""))
                            .path(path)
                            .build()
            );
            if (!path.isEmpty()) {
                applyNavigationPath(robot, path);

            }
            return true;
        } catch (IllegalStateException e) {
            AddWarningDto addWarningDto = new AddWarningDto();
            addWarningDto.setCode("0");
            addWarningDto.setTitle("调度告警");
            addWarningDto.setContent("规划路径失败");
            addWarningDto.setSuggestion("无");
            addWarningDto.setLevel(1);
            SpringUtil.getApplicationContext().publishEvent(addWarningDto);
            log.warn("go station IllegalStateException");
        } catch (Exception e) {
            // 记录更详细的错误信息
            log.error("An error occurred while attempting to move the robot to target position: {}", mapPos, e);
        }
        return false;
    }


    private boolean isSameMap(Robot robot, RobotMapPos mapPos) {
        return robot.getMapCode().equals(mapPos.getMapCode());
    }

    private RobotMapPos findNearestPosition(IRobotPosDto posDto, List<RobotMapPos> mapPositions) {
        return this.nearPos(posDto.getX(), posDto.getY(), mapPositions);
    }

    private List<RobotMapPosDto> applyNavigationPath(Robot robot, List<RobotMapPos> path) {
        return send2ServerService.applyPath(robot, new LinkedList<>(path));
    }

    private RobotMapPos nearPos(float x, float y, List<RobotMapPos> robotMapPosList) throws IllegalStateException {
        if (robotMapPosList.isEmpty()) {
            throw new IllegalArgumentException("输入的列表为空");
        }

        GeometryFactory geometryFactory = new GeometryFactory();
        RobotMapPos nearestPos = null;
        double distance = Double.MAX_VALUE;
        boolean foundValidPosition = false;
        Point point = geometryFactory.createPoint(new Coordinate(x, y));

        for (RobotMapPos pos : robotMapPosList) {
            Point targetPos = geometryFactory.createPoint(new Coordinate(pos.getPosX(), pos.getPosY()));
            double currentDistance = DistanceOp.distance(point, targetPos);
            if (currentDistance < distance) {
                distance = currentDistance;
                nearestPos = pos;
                foundValidPosition = true;
            }
        }

        if (!foundValidPosition) {
            throw new IllegalStateException("没有找到有效的点位，所有距离均超过阈值。");
        }

        //if (distance > 0.3) {
        //    throw new IllegalStateException("最近点位: " + nearestPos.getName() + " 距离: " + distance + ", 超过范围，无法操作");
        //} else {
        //    log.info("nearPosition: {}, and distance {}", nearestPos.getName(), distance);
        //    return nearestPos;
        //}
        return nearestPos;
    }






}
